#include "wearhouse_navigation/wearhouse_navigator.hpp"
#include <jsoncpp/json/json.h>
#include <tf2/LinearMath/Quaternion.h>
#include <cmath>
#include <algorithm>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

namespace wearhouse_navigation {

    WearhouseNavigator::WearhouseNavigator() : Node("wearhouse_navigator") {

        // 声明参数
        correction_interval_ = this->declare_parameter("navigation.correction_interval", 1);
        max_linear_velocity_ = this->declare_parameter("navigation.max_linear_velocity", 0.1);
        max_angular_velocity_ = this->declare_parameter("navigation.max_angular_velocity", 0.1);
        position_tolerance_ = this->declare_parameter("navigation.position_tolerance", 0.1);
        orientation_tolerance_ = this->declare_parameter("navigation.orientation_tolerance", 0.05);

        max_acceleration_ = this->declare_parameter("navigation.max_acceleration", 0.1);
        max_angular_acceleration_ = this->declare_parameter("navigation.max_angular_acceleration", 0.1);

        // 初始化机器人位姿
        current_pose_ = {0.0, 0.0, 0.0};
        has_goal_ = false;
        pose_valid_ = false;
        angle_corrected_ = false;
        correction_counter_ = 0;

        // 创建订阅和发布
        pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
            "robot_pose", 10, std::bind(&WearhouseNavigator::poseCallback, this, std::placeholders::_1));
        

        cmd_vel_pub_ = this->create_publisher<std_msgs::msg::String>("cmd_vel_json", 10);


        // 创建 Action Server
        action_server_ = rclcpp_action::create_server<NavigateToPose>(
            this->shared_from_this(),
            "navigate_to_pose",
            std::bind(&WearhouseNavigator::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&WearhouseNavigator::handle_cancel, this, std::placeholders::_1),
            std::bind(&WearhouseNavigator::handle_accepted, this, std::placeholders::_1)
        );


        RCLCPP_INFO(this->get_logger(), "Wearhouse Navigation initialized with goal: x=%.2f, y=%.2f, theta=%.2f",
                goal_pose_.x, goal_pose_.y, goal_pose_.theta);
    }


    // Action Server回调
    // 1 handle_goal回调函数
    rclcpp_action::GoalResponse WearhouseNavigator::handle_goal(const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const NavigateToPose::Goal> goal) {
        RCLCPP_INFO(this->get_logger(), "Received goal request with ID: %s", std::string(uuid.begin(), uuid.end()).c_str());
        (void)uuid;
        (void)goal;
        // 检查目标点是否有效
        if(std::isnan(goal->target_pose.pose.position.x) || std::isnan(goal->target_pose.posse.position.y)){
            RCLCPP_ERROR(this->get_logger(), "Received invalid goal (NaN coordinates).");
            return rclcpp_action::GoalResponse::REJECT;
        }

        // 检查四元数是否有效
        auto q = goal->target_pose.pose.orientation;
        double length = std::sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
        if(std::abs(length - 1.0) > 1e-3){
            RCLCPP_ERROR(this->get_logger(), "Received invalid goal (non-unit quaternion).");
            return rclcpp_action::GoalResponse::REJECT;
        }

        // 如果当时正在导航，拒绝新目标
        if(is_navigating_) {
            RCLCPP_WARN(this->get_logger(), "Already navigating, rejecting new goal.");
            return rclcpp_action::GoalResponse::REJECT;
        }

        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTIVE;
    }


    // 2 handle cancel回调函数
    rclcpp_action::CancelResponse WearhouseNavigator::handle_cancel(
        const std::shared_ptr<GoalHandleNavigateToPose> goal_handle){
        RCLCPP_INFO(this->get_logger(), "Received request to cancel goal.");
        (void)goal_handle;
        // 如果正在导航，设置标志以便执行函数可以检查并停止
        if(is_navigating_ && goal_handle == active_goal_handle_) {
            // 发送零速度指令
            std_msgs::msg::String msg;
            VelocityJsonConverter json_cmd;
            json_cmd.set_from_twist(0.0, 0.0, 0.0);
            msg.data = json_cmd.get_json_String();
            cmd_vel_pub_->publish(msg);
            return rclcpp_action::CancelResponse::ACCEPT;
        }
        return rclcpp_action::CancelResponse::REJECT: // 如果没有活跃目标，拒绝取消
    }


    // 3 handle accept回调函数
    void WearhouseNavigator::handle_accepted(const std::shared_ptr<GoalHandleNavigateToPose> goal_handle) {
        RCLCPP_INFO(this->get_logger(), "Goal accepted, starting execution.");
        // 启动一个新线程来执行导航逻辑
        std::thread{std::bind(&WearhouseNavigator::execute_navigation, this, std::placeholders::_1), goal_handle}.detach();
    }
    
    // 导航执行线程
    void WearhouseNavigator::execute_navigation(const std::shared_ptr<GoalHandleNavigateToPose> goal_handle) { 
        RCLCPP_INFO(this->get_logger(), "Executing navigation in background thread.");

        // 设置活跃状态
        is_navigating_ = true;
        active_goal_handle_ = goal_handle;

        // 从 Action Goal 获取目标位姿
        auto target_pose_msg = goal_handle->get_goal()->target_pose;
        // 将 geometry_msgs::msg::PoseWithCovarianceStamped 转换为 Pose2D
        tf2::Quaternion q(
            target_pose_msg.pose.orientation.x,
            target_pose_msg.pose.orientation.y,
            target_pose_msg.pose.orientation.z,
            target_pose_msg.pose.orientation.w
        );
        double roll, pitch, yaw;
        tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);

        goal_pose_.x = target_pose_msg.pose.position.x;
        goal_pose_.y = target_pose_msg.pose.position.y;
        goal_pose_.theta = yaw;

        // 重置导航状态
        has_goal_ = true;
        angle_corrected_ = false;
        prev_linear_x_ = 0.0;
        prev_linear_y_ = 0.0;
        prev_angular_z_ = 0.0;

        auto feedback_msg = NavigateToPose::Feedback();
        auto result_msg = NavigateToPose::Result();

        // 导航主循环
        rclcpp::Rate rate(10); // 10Hz，与原来的 timerCallback 频率一致
        while(rclcpp::ok() & is_navigating_)  { 
            if(goal_handle_->is_canceling()) { 
                RCLCPP_INFO(this->get_logger(), "Goal is cancelling, stopping robot.");
                // 发送零速度指令
                std_msgs::msg::String msg;
                VelocityJsonConverter json_cmd;
                json_cmd.set_from_twist(0.0, 0.0, 0.0);
                msg.data = json_cmd.get_json_string();
                cmd_vel_pub_->publish(msg);
                // 设置结果
                result_msg.reached = false;
                result_msg.message = "Goal was canceled.";
                goal_handle_->canceled(result_msg);
                RCLCPP_INFO(this->get_logger(), "Goal was canceled.");
                is_navigating_ = false;
                return;
            }

            if(has_goal_ && pose_valid_) {
                double linear_x = 0.0, linear_y = 0.0, angular_z = 0.0; 
                computeGoalNavigationVelocity(linear_x, linear_y, angular_z);

                // 发布速度指令
                if(!std::isnan(linear_x) && !std::isnan(linear_y) && !std::isnan(angular_z)){
                    std_msgs::msg::String msg;
                    VelocityJsonConverter json_cmd;
                    json_cmd.set_from_twist(linear_x, linear_y, angular_z);
                    std::string json_string = json_cmd.get_json_string();
                    if(!json_string.empty() && json_string != "null"){
                        msg.data = json_string;
                        cmd_vel_pub_->publish(msg);
                    } 
                }

                // 计算距离并发送反馈
                double dx_global = goal_pose_.x - current_pose_.x;
                double dy_global = goal_pose_.y - current_pose_.y;
                double distance = std::sqrt(dx_global*dx_global + dy_global*dy_global);
                feedback_msg.distance_remaining = distance;
                feedback_msg.status = "Navigating...";
                goal_handle_->publish_feedback(feedback_msg);

                // 检查是否已到达目标
                if(!has_goal_){ // computeGoalNavigationCommand内部会设置has_goal_为false
                    result_msg.reached = true;
                    result_msg.message = "Goal has reached successfully.";
                    goal_handle_->succeed(result_msg);
                    RCLCPP_INFO(this->get_logger(), "Goal reached successfully.");
                    is_navigating_ = false;
                    return;
                }
            }
                
            else 
            {
                // 没有有效位姿或没有目标，发送零速度
                std_msgs::msg::String msg;
                VelocityJsonConverter json_cmd;
                json_cmd.set_from_twist(0.0, 0.0, 0.0);
                msg.data = json_cmd.get_json_string();
                cmd_vel_pub_->publish(msg);
                feedback_msg.status = "Waiting for valid pose or goal.";
                goal_handle_->publish_feedback(feedback_msg);
            }

            rate.sleep();   

        }


        // 如果循环因为 rclcpp：：ok() 检测到退出，说明节点被关闭
        if(is_navigating_){
            result_msg.reached = false;
            result_msg.message = "Node shutdown during execution.";
            goal_handle_->abort(result_msg);
            RCLCPP_INFO(this->get_logger(), "Goal aborted due to node shutdown.");
            is_navigating_ = false;
        }
    
    }


    void WearhouseNavigator::poseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {   

        // 1. 检查位置坐标 (x, y) 是否为有效数字 (非 NaN, 非 Inf)
        if (std::isnan(msg->pose.pose.position.x) || std::isinf(msg->pose.pose.position.x) ||
            std::isnan(msg->pose.pose.position.y) || std::isinf(msg->pose.pose.position.y)) {
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                                "Received pose with invalid position (NaN/Inf), ignoring.");
            pose_valid_ = false;
            return;
        }

        // 2. 检查四元数 (orientation) 的分量是否为有效数字 (非 NaN, 非 Inf)
        const auto& q = msg->pose.pose.orientation;
        if (std::isnan(q.x) || std::isinf(q.x) ||
            std::isnan(q.y) || std::isinf(q.y) ||
            std::isnan(q.z) || std::isinf(q.z) ||
            std::isnan(q.w) || std::isinf(q.w)) {
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                                "Received pose with invalid quaternion (NaN/Inf), ignoring.");
            pose_valid_ = false;
            return;
        }

        // 3. 如果所有检查都通过，则更新位姿并标记为有效
        // 从四元数提取角度
        tf2::Quaternion q_tf(
            msg->pose.pose.orientation.x,
            msg->pose.pose.orientation.y,
            msg->pose.pose.orientation.z,
            msg->pose.pose.orientation.w
        );

        // 检查四元数是否有效
        double length = q_tf.length();
        if (std::abs(length - 1.0) < 1e-3) {
            double roll, pitch, yaw;
            tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw);

            current_pose_.x = msg->pose.pose.position.x;
            current_pose_.y = msg->pose.pose.position.y;
            current_pose_.theta = yaw;

            pose_valid_ = true; 
            last_pose_update_time_ = this->get_clock()->now(); 
            
        }
        else {
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                                "Received pose with invalid quaternion, ignoring.");
            pose_valid_ = false;
        }

    }


/*     void WearhouseNavigator::timerCallback() {

        // 计算并发布控制指令
        if(correction_counter_ % correction_interval_ == 0) 
        {
            double linear_x = 0.0, linear_y = 0.0, angular_z = 0.0;
        
            if (has_goal_ && pose_valid_) 
            {
                computeGoalNavigationCommand(linear_x, linear_y, angular_z);
            } 

            
            // 发布json格式的速度指令
            VelocityJsonConverter json_cmd;
            json_cmd.set_from_twist(linear_x, linear_y, angular_z);
            
            std::string json_string = json_cmd.get_json_string();

            if(!json_string.empty() && json_string != "null") {
                std_msgs::msg::String msg;
                msg.data = json_string;
                cmd_vel_pub_->publish(msg);
            }

            correction_counter_ = 0;
        }
        
        correction_counter_++;
    } */


    void WearhouseNavigator::computeGoalNavigationCommand(double& linear_x, double& linear_y, double& angular_z) {
        
        linear_x = 0.0; linear_y = 0.0; angular_z = 0.0;
    
        if (!pose_valid_ || !has_goal_) {
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, 
                                "No valid pose estimate, stopping robot");
            // 应用速度平滑到停止状态
            applyVelocitySmoothing(linear_x, linear_y, angular_z, 0.0, 0.0, 0.0);
            return;  
        }

        auto current_time = this->get_clock()->now();
        auto time_since_last_pose = current_time - last_pose_update_time_;
        if (time_since_last_pose.seconds() > 0.5) { 
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                                "Pose estimate is too old (%.2f s), stopping robot", time_since_last_pose.seconds());
            applyVelocitySmoothing(linear_x, linear_y, angular_z, 0.0, 0.0, 0.0);
            pose_valid_ = false; 
            return;
        }


        // 计算当前与目标点之间的距离
        double dx_global = goal_pose_.x - current_pose_.x;
        double dy_global = goal_pose_.y - current_pose_.y;
        double distance = std::sqrt(dx_global * dx_global + dy_global * dy_global);

        // 计算偏航角
        double orientation_error = goal_pose_.theta - current_pose_.theta;

        // 角度归一化
        while(orientation_error > M_PI)  orientation_error -= 2 * M_PI;
        while(orientation_error < -M_PI) orientation_error += 2 * M_PI;  

        // 检测是否到达目标点：距离与角度差
        bool at_position = distance < position_tolerance_;
        bool at_orientation = std::abs(orientation_error) < orientation_tolerance_;


        if(at_position && at_orientation) {
            has_goal_ = false; // 设置标志，表示任务已经完成
            angle_corrected_ = false; // 为下次导航重置
            RCLCPP_INFO(this->get_logger(), "Goal reached");
            applyVelocitySmoothing(linear_x, linear_y, angular_z, 0.0, 0.0, 0.0);  // 平滑停止
            return;
        }



        // 首先进行角度矫正！
        if(!angle_corrected_ && pose_valid_) {
            // 设置角度矫正阈值
            if(std::abs(orientation_error) > orientation_tolerance_) 
            {
                double target_angular_z = std::clamp(orientation_error * 2.0, -max_angular_velocity_, max_angular_velocity_);

                // 输出角速度进行矫正，此时线速度为0
                applyVelocitySmoothing(linear_x, linear_y, angular_z, 0.0, 0.0, target_angular_z);

                RCLCPP_INFO(this->get_logger(), "Correcting angle: error=%.3f, angular_z=%.3f", 
                            orientation_error, angular_z);
            } 
            else 
            {
                // 若角度已对准，则切换到移动阶段
                angle_corrected_ = true;
                // 设置为0速度，等待下一次循环
                applyVelocitySmoothing(linear_x, linear_y, angular_z, 0.0, 0.0, 0.0);
                RCLCPP_DEBUG(this->get_logger(), "Angle corrected, starting linear movement");

            }

            return;

        }

        // 直线移动阶段
        // 将全局坐标系下的距离向量转换到机器人坐标系下
        double cos_theta = std::cos(current_pose_.theta);
        double sin_theta = std::sin(current_pose_.theta);

        // 旋转矩阵转换坐标差
        double dx_robot = cos_theta * dx_global + sin_theta * dy_global; // 机器人前方分量
        double dy_robot = -sin_theta * dx_global + cos_theta * dy_global; // 机器人左侧分量

        RCLCPP_INFO(this->get_logger(), "计算得到dx_robot=%.3f, dy_robot=%.3f, orientation_error=%.3f", 
                    dx_robot, dy_robot, orientation_error);


        // 优化：当距离很小时，逐渐减小速度而不是突然停止
        double target_linear_x = 0.0, target_linear_y = 0.0; 

        if (distance > 0.1) {  // 避免除零，同时给一个小缓冲区
            // 根据距离调整速度：距离越近，速度越小
            double speed_factor = std::min(1.0, distance / position_tolerance_); // 速度因子：0-1
            target_linear_x = max_linear_velocity_ * speed_factor * (dx_robot / distance);
            target_linear_y = max_linear_velocity_ * speed_factor * (dy_robot / distance);


        } else {
            // 非常接近目标时，使用最小速度或停止
            target_linear_x = 0.0;
            target_linear_y = 0.0;
        }

        // 应用速度平滑
        applyVelocitySmoothing(linear_x, linear_y, angular_z, target_linear_x, target_linear_y, 0.0);


        // 检查是否需要重新进行角度矫正（由于运行导致的角度偏差）
        if(std::abs(orientation_error) > orientation_tolerance_) {
            RCLCPP_DEBUG(this->get_logger(), "Movement deviation detected, re-correcting angle!");
            angle_corrected_ = false;
            return;
        }

        RCLCPP_INFO(this->get_logger(), "Moving directly with speed linear_x=%.3f, linear_y=%.3f, distance=%.3f", 
                    linear_x, linear_y, distance);

    }

    // 速度平滑函数
    void WearhouseNavigator::applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                            double target_linear_x, double target_linear_y, double target_angular_z) {
        
        // 计算时间间隔:100ms 
        double dt = 0.1; 
        
        // 计算线性速度的最大变化量
        double max_linear_change = max_acceleration_ * dt;
        double max_angular_change = max_angular_acceleration_ * dt;
        
        // 计算实际速度变化（限制在最大变化范围内）
        double dx_change = std::clamp(target_linear_x - prev_linear_x_, 
                                    -max_linear_change, max_linear_change);
        double dy_change = std::clamp(target_linear_y - prev_linear_y_, 
                                    -max_linear_change, max_linear_change);
        double dtheta_change = std::clamp(target_angular_z - prev_angular_z_, 
                                        -max_angular_change, max_angular_change);
        
        // 更新目标速度
        linear_x = prev_linear_x_ + dx_change;
        linear_y = prev_linear_y_ + dy_change;
        angular_z = prev_angular_z_ + dtheta_change;
        
        // 更新上一次的速度值
        prev_linear_x_ = linear_x;
        prev_linear_y_ = linear_y;
        prev_angular_z_ = angular_z;
    }

    // 距离计算函数
    double WearhouseNavigator::pointToLineDistance(const Eigen::Vector2d& point, const Line& line) {
        if(line.a == 0 && line.b == 0) return 0;
        return std::abs(line.a * point.x() + line.b * point.y() + line.c) /
               std::sqrt(line.a * line.a + line.b * line.b);
    }



} // namespace wearhouse_navigation

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<wearhouse_navigation::WearhouseNavigator>());
    rclcpp::shutdown();
    return 0;
}